#include "PID.h"


TARGET_typeDef target = {0};

extern OpenMV_typedef openmv;
extern Timer_typeDef timer;

//extern float Pitch,Yaw,Roll,yaw_target;
extern float fAngle[3], yaw_target;
//extern int GyroZ;


PID_InitDefStruct p_ang=		
{
	.Kp = 65,	//11.15
	.Ki = 0.00001,
	.Kd = 0,  
	.Ur = 9999,
	.Ur_d = -9999,
	.U_en = 1,	
	.PID_is_Enable = 1,	
	.Un = 0,			
	.En_1 = 0,	 
	.En_2 = 0,   
	.En_All = 0,
	.PWM = 0
};


PID_InitDefStruct p_gray=		
{
	.Kp = 1000,
	.Ki = 0,
	.Kd = 0,  
	.Ur = 6999,
	.Ur_d = -6999,	 
	.PID_is_Enable = 1,	
	.Un = 0,			
	.En_1 = 0,	 
	.En_2 = 0,   
	.En_All = 0,
	.PWM = 0
};


PID_InitDefStruct p_gray_turn=		
{
	.Kp = 2150,			// 1500
	.Ki = 0.005,
	.Kd = 0,  
	.Ur = 2999,
	.Ur_d = -14999,	 
	.PID_is_Enable = 1,	
	.Un = 0,			
	.En_1 = 0,	 
	.En_2 = 0,   
	.En_All = 0,
	.PWM = 0
};


void PIDCal_Start(void)
{
	timer.state = STATE_PID_MODE;
}

void PIDCal_End(void)
{
	timer.state = STATE_OPEN_LOOP;
}

/*--------------------------------------------------
输入参数：目标速度,当前速度,pid系数结构体
功能：速度环的控制函数（增量式）
----------------------------------------------------*/
//int PID_Velocity(float TargetVelocity,float CurrentVelocity,PID_InitDefStruct* p)
//{
//	if(p->PID_is_Enable == 1)
//	{
//		float En = TargetVelocity - CurrentVelocity;//error

//		if(En <= p->U_en && En >= p->U_en )			//允许误差
//			return 0;
//			
//		p->Un += p->Kp*(En - p->En_1) + p->Ki*En + p->Kd*(En - 2*p->En_1 + p->En_2);
//		
//		p->En_2=p->En_1;
//		p->En_1=En;
//		
//		p->PWM = p->Un;
//		
//		if(p->PWM>p->Ur) p->PWM=p->Ur;				//输出限额
//		if(p->PWM<-p->Ur) p->PWM=-p->Ur;
//	}
//	else
//	{
//		p->PID_is_Enable = 1;
//	}
//	return p->PWM;									//由PID计算好的PWM值
//}

/*--------------------------------------------------
输入参数：目标累计圈数,当前累计圈数,pid系数结构体
功能：位置环的控制函数
----------------------------------------------------*/
//int PID_Position(int TargetCul,int CurrentCul,PID_InitDefStruct* p)
//{
//	if(p->PID_is_Enable == 1)
//	{
//		p->En_1 = TargetCul - CurrentCul  ;	//now error
//		
////		if(p->En_1 <= p->U_en && p->En_1 >= -p->U_en )			//允许误差
//		if(p->En_1 <=0 && p->En_1 >= -p->U_en)
//			return 0;
//		
//		p->En_All += p->En_1;     			//accumulated error
//	
//		p->PWM = p->Kp*p->En_1 + p->Ki/100*p->En_All + p->Kd*(p->En_1 - p->En_2);		
//		
//		p->En_2 = p->En_1;					//last error
//		
//		if(p->PWM>p->Ur)  p->PWM = p->Ur;	//输出限额
//		if(p->PWM<-p->Ur) p->PWM =-p->Ur;
//	}
//	else
//	{
//		p->PID_is_Enable = 1;
//	}
//	return p->PWM;							//由PID计算好的PWM值
//}

/*--------------------------------------------------
输入参数：角度，平衡角度，角加速度
功能：直立环(or角度环)的控制函数 
----------------------------------------------------*/
//int PID_Angle_1(float angle,float balance_angle,int Gyro,PID_InitDefStruct* p)
//{		
//	float error = angle - balance_angle;
//	if(p->PID_is_Enable == 1)
//	{
//		p->En_All += error;
//		p->PWM = p->Kp*error + p->Ki/10*p->En_All + p->Kd*Gyro;
//		
//		if(p->PWM > p->Ur)	p->PWM = p->Ur;		//输出限额
//		if(p->PWM <-p->Ur) 	p->PWM =-p->Ur;
//	}
//	else
//	{
//		p->PID_is_Enable = 1;
//		p->En_All = 0;
//	}
//	return p->PWM;
//}

int PID_Angle_2(float angle,float balance_angle,PID_InitDefStruct* p)
{		
	float error = angle - balance_angle;
	if(p->PID_is_Enable == 1)
	{
		p->En_All += error;
		p->PWM = p->Kp*error + p->Ki/10*p->En_All + p->Kd*(error - 2*p->En_1 + p->En_2);

		p->En_2 = p->En_1;
		p->En_1 = error;
		
		if(p->PWM > p->Ur)	p->PWM = p->Ur;		//输出限额
		if(p->PWM <-p->Ur) 	p->PWM =-p->Ur;
	}
	else
	{
		p->PID_is_Enable = 1;
		p->En_All = 0;
	}
	return p->PWM;
}

/*--------------------------------------------------
输入参数：左轮速度，右轮速度，角加速度
功能：转向环的控制函数
注意：公式中各系数正负需要依靠试验确定
----------------------------------------------------*/
//int PID_Veer(float encoder_left, float encoder_right, int Gyro,PID_InitDefStruct* p)
//{
//	int Turn_Amplitude=9999; 				//误差限幅
//	
//	p->En_1 = encoder_left - encoder_right;	//当次误差
//	p->En_2 += p->En_1; 					//对角速度积分，累计误差
//	
//	if(p->En_2 > Turn_Amplitude)  			//限幅
//    	p->En_2 = Turn_Amplitude;
//	if(p->En_2 < -Turn_Amplitude) 
//		p->En_2 = -Turn_Amplitude;
//	
//	p->PWM = p->Kp * p->En_2 - p->Kd * Gyro; //结合Z轴陀螺仪进行PD控制
//	
//	if(p->PWM > p->Ur)  					//限幅
//    	p->PWM = p->Ur;
//	if(p->PWM < -p->Ur) 
//		p->PWM = -p->Ur;

//	return p->PWM;
//}

/*--------------------------------------------------
输入参数：目标速度,当前速度,pid系数结构体
功能：灰度PID
注意：公式中各系数正负需要依靠试验确定
----------------------------------------------------*/
 int PID_GrayScale(PID_InitDefStruct* p)
 {
 	if(p->PID_is_Enable == 1)
 	{
 		p->En_1 = GRAY_Error();		//now error
 		p->En_All += p->En_1;		//accumulated error
	
 		p->Un = p->Kp*p->En_1 + p->Ki*p->En_All + p->Kd*(p->En_1 - p->En_2);		
		
 		p->En_2 = p->En_1;			//last error
		
 		p->PWM = p->Un;
		
 		if(p->PWM>p->Ur) p->PWM=p->Ur;		
 		if(p->PWM<p->Ur_d) p->PWM=p->Ur_d;
 	}
 	else
 	{
 		p->PID_is_Enable = 1;
 		p->En_All = 0;
 	}
 	return p->PWM;				
 }

/*--------------------------------------------------
输入参数：角度，目标角度，角加速度
功能：舵机位置式PID
注意：公式中各系数正负需要依靠试验确定
----------------------------------------------------*/
//int PID_Steer(float Angle, float TargetAngle, int Gyro,PID_InitDefStruct* p)
//{
//	p->En_1 = Angle - TargetAngle;	//当次误差
//	p->En_All += p->En_1; 			//累计误差
//	
////	if(p->En_1 <3 && p->En_1 >-3)  	//
////    p->En_1 = 0;
//	
//	if(p->En_All > p->Ur_i*p->Ur)  	//积分限幅
//    	p->En_All = p->Ur_i;
//	if(p->En_All < -p->Ur_i*p->Ur) 
//		p->En_All = -p->Ur_i;
//	
//	p->PWM = p->Kp*p->En_1 + p->Ki*p->En_All + p->Kd*Gyro;
// 	
//	p->En_2 = p->En_1;
//	
//	if(p->PWM > 48)  				//输出限幅
//    	p->PWM = 48;
//	if(p->PWM < -172) 
//		p->PWM = -172;

//	return p->PWM;
//}

/*--------------------------------------------------
输入参数：角度，目标角度，角加速度
功能：舵机增量式PID
注意：公式中各系数正负需要依靠试验确定
----------------------------------------------------*/
//int PID_Steer_Incremental(float Angle, float TargetAngle, PID_InitDefStruct* p)
//{	
//	float En = TargetAngle - Angle  ;//error                                                     

//	p->Un += p->Kp*(En - p->En_1) + p->Ki*En + p->Kd*(En - 2*p->En_1 + p->En_2);	//Velocity PID
//	
//	p->En_2=p->En_1;
//	p->En_1=En;
//	
//	p->PWM = p->Un;
//	
//	if(p->PWM > 1250)  				//输出限幅
//		p->PWM = 1250;
//	if(p->PWM < 250) 
//		p->PWM = 250;

//	return p->PWM;		
//}

/*--------------------------------------------------
输入参数：坐标，目标坐标，距离
功能：舵机跟随PID
注意：公式中各系数正负需要依靠试验确定
----------------------------------------------------*/
//int PID_Steer_Follow(float Coord, float Target_Coord, 
//										 float Distance, PID_InitDefStruct* p)
//{
//	float En = Distance*(Coord - Target_Coord);	//error  
//	
//	p->Un += p->Kp*(En - p->En_1) + p->Ki*En + p->Kd*(En - 2*p->En_1 + p->En_2);
// 	
//	p->En_2 = p->En_1;
//	p->En_1 = En;
//	
//	p->PWM = p->Un;
//	
//	if(p->PWM > p->Ur)  						//输出限幅
//    	p->PWM = p->Ur;
//	if(p->PWM < p->Ur_d) 
//		p->PWM = p->Ur_d;

//	return p->PWM + 750;
//}


/* closed loop		outer loop->Position	inner->Velocity */
//int PID_Pos_Vel(int TargetC,int CurrentC,PID_InitDefStruct* Pp,
//				int CurrentV,PID_InitDefStruct* Pv)
//{
//	Pp->PWM = PID_Position(TargetC, CurrentC, Pp);
//	Pv->PWM = PID_Velocity(Pp->PWM, CurrentV, Pv);
//	
//	return Pv->PWM;
//}


int PWM = 0;

void PID_ModeChoose(PID_Mode mode)
{
    switch (mode)
    {
        case OPEN_LOOP:     
            p_ang.En_All = 0;
            	break;

        case GRAYSCALE:    
			PWM = PID_GrayScale(&p_gray);
			MOTOR_GiveSpeed(4000-PWM, 4000+PWM);
            	break;
				
		case GRAYSCALE_TURN:    
			PWM = PID_GrayScale(&p_gray_turn);
			MOTOR_GiveSpeed(4000-PWM, 4000+PWM);
            	break;

        case ANGLE_Left:         
			PWM = PID_Angle_2(fAngle[2], target.yaw, &p_ang);
			MOTOR_GiveSpeed(+PWM, -PWM);
            	break;

		case ANGLE_Right:         
			// PWM = PID_Angle(Yaw, Target_Angle, GyroZ, &p_ang);
			// MOTOR_GiveSpeed(+PWM, -PWM);
            	break;

        case SPEED:         

            	break;

        case POSITION:      

            	break;

        case SPEED_POSITION_DUAL_LOOP:  
			// int PWM1 = PID_Pos_Vel(Target1_N,ENCODER1_N,&pp,ENCODER1_V,&pv1);
			// int PWM2 = PID_Pos_Vel(Target2_N,ENCODER2_N,&pp,ENCODER2_V,&pv1);
			// if(ENCODER1_N>=Target1_N-2 && ENCODER1_N<=Target1_N+2 && 		//去除最后抖动
			// 	 ENCODER2_N>=Target2_N-2 && ENCODER2_N<=Target2_N+2)
			// {
			// 	PWM1 = 0;
			// 	PWM2 = 0;
			// }	
			// MOTOR_GiveSpeed(PWM1, PWM2);
            	break;

        default:
            break;
    }
}

